﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Features2D;
using Emgu.CV.Util;
using Emgu.Util;
using Emgu.CV.CvEnum;
using Emgu.CV.UI;


namespace NAO_Image_Processing
{
    class ImageProcessor
    {
        int circleRadius;
        int circleThickness;
        int resolutionHeigth;
        int resolutionWidth;
        double entfBildflaeche;
        Size textPosA;
        Size textPosC;
        double textScale;
        double textThickness;
        double pixelProCMX;
        double pixelProCMY;
        CircleF kreisBall;
        /*Farben
         * B - G - R
         */
        MCvScalar farbeRot = new MCvScalar(0, 0, 255);
        MCvScalar farbeGruen = new MCvScalar(0, 255, 0);
        MCvScalar farbeBlau = new MCvScalar(255, 0, 0);
        MCvScalar farbeWeiss = new MCvScalar(255, 255, 255);
        static double[, ,] posBildflaechePixel = new double[10, 30, 2];

        public ImageProcessor(int[] resolution)
        {
            this.resolutionWidth = resolution[0];
            this.resolutionHeigth = resolution[1];
            setScreenParameters(resolutionHeigth);
        }

        public Image<Bgr, Byte> zeichneEntfernungen(Image<Bgr, Byte> inputImageC, List<double[, ,]> values, int resolutionHeigth)
        {
            double[, ,] distanceValuesScreen = values.ElementAt(0);
            double[, ,] distanceValuesFB = values.ElementAt(1);
            double[,] distanceValuesFBText = new double[10,2];
            List<Point> points = new List<Point>();
            this.resolutionHeigth = resolutionHeigth;
            this.resolutionWidth = (int)(resolutionHeigth*(4.0f/3.0f));
            entfBildflaeche = distanceValuesFB[9, 0, 0];
            pixelProCMX = (resolutionWidth / 2) / distanceValuesScreen[0, 0, 1];
            pixelProCMY = (resolutionHeigth / 2) / distanceValuesScreen[0, 0, 1];

            /* Berechnung der Position der Kreispunkte auf der Bildebene */
            for (int i = 0; i < 10; i++)
            {
                for (int j = 0; j < 30; j++)
                {
                    Matrix temp = konvertierePositionInPixel(distanceValuesScreen[i, j, 0], distanceValuesScreen[i, j, 1]);
                    posBildflaechePixel[i, j, 0] = temp[0, 0];
                    posBildflaechePixel[i, j, 1] = temp[1, 0];
                    points.Add(new Point((int)posBildflaechePixel[i, j, 0], (int)posBildflaechePixel[i, j, 1]));
                }
            }

            /*Punkte der Linie vertikalen Linie*/
            Point point_top = points[0];
            Point point_bottom = points[points.Count - 1];
            /*P*/
            MCvFont font = new MCvFont();
            CvInvoke.cvInitFont(ref font,
                                Emgu.CV.CvEnum.FONT.CV_FONT_HERSHEY_COMPLEX,
                                textScale,
                                textScale,
                                textThickness,
                                0,
                                Emgu.CV.CvEnum.LINE_TYPE.CV_AA);
            /*Punkte einzeichnen*/
            for (int i = 0; i < points.Count; i++)
            {
                CvInvoke.cvCircle(inputImageC, //Bild für Operation
                                  points[i], //Kreismittelpunkt
                                  circleRadius, //Radius
                                  farbeWeiss, //Farbe
                                  circleThickness, //Dicke des Rands
                                  Emgu.CV.CvEnum.LINE_TYPE.CV_AA, //Linientyp 
                                  0 //Shift
                                  );
                CvInvoke.cvCircle(inputImageC, //Bild für Operation
                                  new Point(resolutionWidth-points[i].X,points[i].Y), //Kreismittelpunkt
                                  circleRadius, //Radius
                                  farbeWeiss, //Farbe (Grün)
                                  circleThickness, //Dicke des Rands
                                  Emgu.CV.CvEnum.LINE_TYPE.CV_AA, //Linientyp 
                                  0 //Shift
                                  );
                
            }
            /*Entfernungstext einzeichnen*/
            for (int i = 0; i < 10; i++)
            {
                CvInvoke.cvPutText(inputImageC, //Bild für Operation
                                  (int)(distanceValuesFB[i, 0, 0]) + " cm", //Ausgabetext
                                  Point.Add(new Point((int)posBildflaechePixel[i, 0, 0], (int)posBildflaechePixel[i, 0, 1]), textPosA), //Position des Textes (Nach links oben versetzt)
                                  ref font, //Textart
                                  farbeWeiss //Farbe (Weiss)
                                  );
            }
            /*Linie einzeichnen*/
            CvInvoke.cvLine(inputImageC, point_top, point_bottom, new MCvScalar(255, 255, 255), 1, LINE_TYPE.CV_AA, 0);
            return inputImageC;
        }

        public Image<Bgr, Byte> SURF(Image<Bgr, Byte> inputImageC, Image<Bgr, Byte> outputImageC,double camHeigth)
        {
            /*SURF*/
            double[] bodenKoordinaten = new double[2];
            double[] posAufBildflaeche = new double[2];
            Image<Gray, Byte> inputImageG = inputImageC.Convert<Gray, Byte>();
            SURFDetector surfCPU = new SURFDetector(1000, false);
            VectorOfKeyPoint modelKeyPointsSURF = new VectorOfKeyPoint();
            Matrix<float> modelDescriptorsSURF = surfCPU.DetectAndCompute(inputImageG, null, modelKeyPointsSURF);
            MKeyPoint[] SURFPoints = modelKeyPointsSURF.ToArray();
            /*Keypoints für die untere Bildhälfte [RoI] in das Ausgangsbild für SURF einzeichnen*/
            foreach (MKeyPoint m in SURFPoints)
	        {
                if (m.Point.Y > (int)(outputImageC.Height / 2.0f))
                {
                    posAufBildflaeche[0] = (int)m.Point.X;
                    posAufBildflaeche[1] = (int)m.Point.Y;
                    posAufBildflaeche = pixelNachPositionAufBildflaeche(posAufBildflaeche);
                    bodenKoordinaten = positionAufBildflaecheNachEntfernung(posAufBildflaeche, camHeigth);
                    //outputImageC = zeichneEntfernungZuPunkt(outputImageC, new Point((int)m.Point.X,(int)m.Point.Y), NAOControl.euklidDistance(bodenKoordinaten[0],bodenKoordinaten[1]));
                    outputImageC = zeichnePunkt(outputImageC, null, new Point((int)m.Point.X, (int)m.Point.Y), 0);
                    //outputImageC = zeichnePunkt(outputImageC, null, new Point((int)m.Point.X, (int)m.Point.Y), NAOControl.euklidDistance(bodenKoordinaten[0], bodenKoordinaten[1]));
                }
        	}
            return outputImageC;
        }

        public Image<Bgr, Byte> HoughLines(Image<Bgr, Byte> inputImageC, Image<Bgr, Byte> outputImageC)
        {
            LineSegment2D[][] lines = inputImageC.HoughLines(150, 250, 1.0, Math.PI / 180, 20, 10, 10);
            for (int i = 0; i < lines[0].Length; i++)
            {
                CvInvoke.cvLine(outputImageC.Ptr,
                                lines[0][i].P1,
                                lines[0][i].P2,
                                farbeRot,
                                1,
                                LINE_TYPE.CV_AA,
                                0);
            }

            return outputImageC;
        }

        public Image<Bgr, Byte> zeichnePunkt(Image<Bgr, Byte> destImage, Image<Bgr, Byte> overlayImage, Point koordinaten, double entfernung = 0)
        {
            /*Font definieren*/
            MCvFont font = new MCvFont();
            CvInvoke.cvInitFont(ref font,
                                Emgu.CV.CvEnum.FONT.CV_FONT_HERSHEY_COMPLEX,
                                textScale,
                                textScale,
                                textThickness,
                                0,
                                Emgu.CV.CvEnum.LINE_TYPE.CV_AA);
            if (overlayImage != null)
            {
                /*Punkt einzeichnen*/
                CvInvoke.cvCircle(overlayImage, //Bild für Operation
                                      koordinaten,//Kreismittelpunkt
                                      circleRadius, //Radius
                                      farbeRot, //Farbe
                                      circleThickness, //Dicke des Rands
                                      Emgu.CV.CvEnum.LINE_TYPE.CV_AA, //Linientyp 
                                      0 //Shift
                                      );
                /*Entfernungstext einzeichnen*/
                if (entfernung != 0)
                {
                    CvInvoke.cvPutText(overlayImage, //Bild für Operation
                                          (int)(entfernung) + " cm", //Ausgabetext
                                          Point.Add(koordinaten, textPosA), //Position des Textes (Nach links oben versetzt)
                                          ref font, //Textart
                                          farbeRot //Farbe
                                          );
                }
                CvInvoke.cvAddWeighted(destImage, 1.0, overlayImage, 1.0, 0.0, destImage);
            }
            else
            {
                /*Punkt einzeichnen*/
                CvInvoke.cvCircle(destImage, //Bild für Operation
                                      koordinaten,//Kreismittelpunkt
                                      circleRadius, //Radius
                                      farbeRot, //Farbe
                                      circleThickness, //Dicke des Rands
                                      Emgu.CV.CvEnum.LINE_TYPE.CV_AA, //Linientyp 
                                      0 //Shift
                                      );
                /*Entfernungstext einzeichnen*/
                if (entfernung != 0)
                {
                    CvInvoke.cvPutText(destImage, //Bild für Operation
                                          (int)(entfernung) + " cm", //Ausgabetext
                                          Point.Add(koordinaten, textPosA), //Position des Textes (Nach links oben versetzt)
                                          ref font, //Textart
                                          farbeRot //Farbe
                                          );
                }
            }
            return destImage;
        }

        public Image<Bgr, Byte> zeichneHoughKreise(IntPtr img, Image<Bgr,Byte> imgOutputC)
        {
            CvInvoke.cvSmooth(img, img, SMOOTH_TYPE.CV_GAUSSIAN, 9, 9, 9, 9);
            Image<Gray, Byte> imgThreshold = new Image<Gray, Byte>(CvInvoke.cvGetSize(img));
            CvInvoke.cvCopy(img, imgThreshold, IntPtr.Zero);
            
            CircleF[] circles = imgThreshold.HoughCircles(new Gray(10), new Gray(50), 2, imgThreshold.Height / 4, 30, 200)[0];
            
            foreach (CircleF kreis in circles)
            {
                if (kreis.Center.Y > imgThreshold.Height / 2)
                {
                    kreisBall = kreis;
                    CvInvoke.cvCircle(imgOutputC, new Point((int)kreis.Center.X, (int)kreis.Center.Y), 3, farbeWeiss, -1, LINE_TYPE.CV_AA, 0);
                    imgOutputC.Draw(kreis, new Bgr(Color.Cyan), 3);
                }
            }
            return imgOutputC;
        }

        public Matrix konvertierePositionInPixel(double x, double y)
        {
            Matrix retKoordinaten = new Matrix(2, 1);
            if (y >= 0)
            {
                /*x-Koordinate*/
                retKoordinaten[0, 0] = (int)((resolutionWidth / 2) + (x * pixelProCMX));
                /*y-Koordinate*/
                retKoordinaten[1, 0] = (int)((resolutionHeigth - y * pixelProCMY));
                return retKoordinaten;
            }
            else
            {
                retKoordinaten[0, 0] = resolutionWidth / 2;
                retKoordinaten[1, 0] = resolutionHeigth;
                return retKoordinaten;
            }
        }

        public double[] pixelNachPositionAufBildflaeche(double[] pixelkoordinaten)
        {
            double[] _pixelkoordinaten = new double[2];
            pixelkoordinaten.CopyTo(_pixelkoordinaten, 0);
            double[] bildkoordinaten = new double[2];
            /* bildkoordinaten[0] = X
             * bildkoordinaten[1] = Y
             */
            if (_pixelkoordinaten[0] < (resolutionWidth / 2.0f))
            {
                _pixelkoordinaten[0] += ((resolutionWidth / 2.0f) - _pixelkoordinaten[0]) * 2.0f;
                bildkoordinaten[0] = -(_pixelkoordinaten[0] - (resolutionWidth / 2.0f)) / (pixelProCMX);
            }
            else
            {
                bildkoordinaten[0] = (_pixelkoordinaten[0] - (resolutionWidth / 2.0f)) / (pixelProCMX);
            }
            bildkoordinaten[1] = (resolutionHeigth - _pixelkoordinaten[1]) / (pixelProCMY);
            return bildkoordinaten;
        }
        
        public double[] positionAufBildflaecheNachEntfernung(double[] bildkoordinaten, double kameraHoehe)
        {
            double[] entfernungskoordinaten = new double[2];
            Matrix p1 = new Matrix(3, 1);
            Matrix p2 = new Matrix(3, 1);
            Matrix g1 = new Matrix(3, 1);
            Matrix g2 = new Matrix(3, 1);
            Matrix entfernung = new Matrix(3, 1);
            double r;
            p1[0, 0] = 0; p1[1, 0] = kameraHoehe; p1[2, 0] = 0;
            p2[0, 0] = entfBildflaeche; p2[1, 0] = bildkoordinaten[1]; p2[2, 0] = bildkoordinaten[0];
            g1=p1;
            g2=p2-p1;
            r = g1[1, 0] / g2[1, 0];
            entfernung = p1 + r * p2;
            entfernungskoordinaten[0] = -entfernung[2, 0];
            entfernungskoordinaten[1] = Math.Abs(entfernung[0, 0]);
            return entfernungskoordinaten;
        }

        public IntPtr holeSchwellwertBild(IntPtr img)
        {
            IntPtr imgHSV = CvInvoke.cvCreateImage(CvInvoke.cvGetSize(img), IPL_DEPTH.IPL_DEPTH_8U, 3);
            CvInvoke.cvCvtColor(img, imgHSV, COLOR_CONVERSION.CV_BGR2HSV);
            IntPtr imgThreshold = CvInvoke.cvCreateImage(CvInvoke.cvGetSize(imgHSV), IPL_DEPTH.IPL_DEPTH_8U, 1);
            CvInvoke.cvInRangeS(imgHSV, new MCvScalar(5, 170, 50), new MCvScalar(50, 255, 255), imgThreshold);
            return imgThreshold;
        }

        public void setScreenParameters(int resolutionHeigth)
        {
            switch (resolutionHeigth)
            {
                case 120: 
                    {
                        circleRadius = 1;
                        circleThickness = 1;
                        textPosA = new Size(-32, -3);
                        textPosC = new Size(10, -3);
                        textScale = 0.18;
                        textThickness = 0;
                        break;
                    }
                case 240:
                    {
                        circleRadius = 2;
                        circleThickness = 1;
                        textPosA = new Size(-40, -3);
                        textPosC = new Size(10, -3);
                        textScale = 0.23;
                        textThickness = 0;
                        break;
                    }
                case 480:
                    {
                        circleRadius = 4;
                        circleThickness = 1;
                        textPosA = new Size(-75, -3);
                        textPosC = new Size(15, -3);
                        textScale = 0.5;
                        textThickness = 0;
                        break;
                    }
                case 960:
                    {
                        circleRadius = 2;
                        circleThickness = 2;
                        textPosA = new Size(-175, -3);
                        textPosC = new Size(40, -3);
                        textScale = 1.25;
                        textThickness = 2;
                        break;
                    }
                default: break;
            }
        }

        public double getEntfBildflaeche()
        {
            return entfBildflaeche;
        }

        public CircleF getKreisBall() 
        {
            return kreisBall;
        }
    }
}
